#!/usr/bin/env python3
import rospy
import threading
from sensor_msgs.msg import Joy
import command
import time
import LED
import config

SLEEP_TIME=0.05

class JoystickDemo:
    def __init__(self):
        rospy.init_node('joystick_demo')
        
        self.axes = [0.0] * 8
        self.buttons = [0] * 12
        self.lock = threading.Lock()
        self.led=LED.LED()
        self.led_status=0

        self.snake_speed=config.SNAKE_SPEED
        self.speed_threshold=config.SPEED_THRESHOLD
        self.ANGLE=config.ANGLE_THRESHOLD
        
        self.running = True

        self.LED_state=False#False表示关，True表示开
        self.Tail_state=False
        self.Mode_state=False#False代表速度模式（默认），True代表位置模式

        self.button_actions = {
            8: {"name": "disable", "active": False, "handler": self.Disable},
            9: {"name": "enable", "active": False, "handler": self.Enable},
            4: {"name": "Straight", "active": False, "handler": self.SnakeToStraight},#LB按键
            5: {"name": "Tail", "active": False, "handler": self.Tail},#RB按键
            7: {"name": "LED", "active": False, "handler": self.LED},#RT按键
            6: {"name":"ChangeMode","active":False,"handler":self.Mode},#LT按键
        }

        self.axis_actions={
            4:{
                1.0:{"name":"Joint_left","active":False,"handler":self.Joint_Left},
                -1.0:{"name":"Joint_right","active":False,"handler":self.Joint_Right}
            },
            5: {
                1.0: {"name": "Joint_up", "active": False, "handler": self.Joint_Up},
                -1.0: {"name": "Joint_down", "active": False, "handler": self.Joint_Down}
            }
        }
        
        self.frontback_thread = threading.Thread(target=self.axis_monitor_task)
        self.leftright_thread = threading.Thread(target=self.right_left_task)
        self.up_thread=threading.Thread(target=self.ThreeUp)
        self.down_thread=threading.Thread(target=self.ThreeDown)
        self.button_thread=threading.Thread(target=self.button_handler)
        self.frontback_thread.daemon = True
        self.leftright_thread.daemon = True
        self.up_thread.daemon=True
        self.down_thread.daemon=True
        self.button_thread.daemon=True

        self.frontback_active = False
        self.leftright_active = False
        self.up_active=False
        self.down_active=False

        self.frontback_thread.start()
        self.leftright_thread.start()
        self.up_thread.start()
        self.down_thread.start()
        self.button_thread.start()
        
        self.robot = command.Robot()

        rospy.Subscriber("joy", Joy, self.joy_callback)

        rospy.on_shutdown(self.shutdown)

    def joy_callback(self, data):
        with self.lock:
            self.axes = data.axes
            self.buttons=data.buttons
            
        # 设置激活标志
        self.frontback_active = abs(data.axes[1]) > 0.1
        self.leftright_active = abs(data.axes[2]) > 0.1
        self.up_active=(self.buttons[3]==1)
        self.down_active=(self.buttons[1]==1)
        for btn_id, action in self.button_actions.items():
            action["active"] = (data.buttons[btn_id] == 1)


        for axis_id, actions in self.axis_actions.items():
            axis_value = data.axes[axis_id]
            for value, action in actions.items():
                action["active"] = (axis_value == value)#严格等于1.0或-1.0时才触发


    def axis_monitor_task(self):
        last_speed = None
        rospy.loginfo("前后移动线程已启动")
    
        while not rospy.is_shutdown():
            if not self.frontback_active:
                if last_speed is not None:
                    rospy.loginfo("履带蛇停止运动")
                    self.robot.stop_move()
                    last_speed = None
                rospy.sleep(SLEEP_TIME)
                continue

            with self.lock:
                axis_value = self.axes[1]

            #线性映射到 -5000~5000
            raw_speed = axis_value * self.snake_speed
            
            #以200为档位调整（减少波动）
            if raw_speed != 0:
                target_speed = int(round(raw_speed / self.speed_threshold) * self.speed_threshold)
            else:
                target_speed = 0
            
            #速度变化时更新
            if target_speed != last_speed:
                if target_speed > 0:
                    rospy.loginfo(f"履带蛇前进，速度: {target_speed}")
                    self.robot.move_front(motor_name=1, speed=target_speed)
                elif target_speed < 0:
                    rospy.loginfo(f"履带蛇后退，速度: {target_speed}")
                    self.robot.move_front(motor_name=1, speed=target_speed)
                else:
                    rospy.loginfo("履带蛇停止")
                    self.robot.move_front(motor_name=1, speed=target_speed)
                last_speed = target_speed
            
            rospy.sleep(SLEEP_TIME)

    def right_left_task(self):
        """左右转向监控线程"""
        has_sent = False
        rospy.loginfo("左右转向线程已启动")
        
        while not rospy.is_shutdown():
            if not self.leftright_active:
                if has_sent:
                    self.robot.move_joint_speed(2, 0)
                    self.robot.move_joint_speed(5, 0)
                    rospy.loginfo("履带蛇停止左右转")
                    has_sent = False
                rospy.sleep(SLEEP_TIME)
                continue
            
            with self.lock:
                axis_value1 = self.axes[2]
            
            if not has_sent:    
                if axis_value1 > 0:
                    rospy.loginfo("履带蛇右转")
                    self.robot.move_joint_speed(2, -1)
                    self.robot.move_joint_speed(5, 1)
                elif axis_value1 < 0:
                    rospy.loginfo("履带蛇左转")
                    self.robot.move_joint_speed(2, 1)
                    self.robot.move_joint_speed(5, -1)
                has_sent = True
            rospy.sleep(SLEEP_TIME)


    def ThreeUp(self):
        has_sent=False
        rospy.loginfo("抬头线程已启动")
        while not rospy.is_shutdown():
            if not self.up_active:
                if has_sent:
                    self.robot.move_joint_speed(3, 0)
                    rospy.loginfo("停止抬头")
                    has_sent = False
                rospy.sleep(SLEEP_TIME)
                continue
            with self.lock:
                up_state=self.buttons[3]
            if up_state==1:
                if not has_sent:
                    rospy.loginfo("履带蛇抬头")
                    self.robot.move_joint_speed(3,1)
                    has_sent=True
            rospy.sleep(SLEEP_TIME)
    
    def ThreeDown(self):
        has_sent=False
        rospy.loginfo("低头线程已启动")
        while not rospy.is_shutdown():
            if not self.down_active:
                if has_sent:
                    self.robot.move_joint_speed(3, 0)
                    rospy.loginfo("停止低头")
                    has_sent = False
                rospy.sleep(SLEEP_TIME)
                continue
            with self.lock:
                down_state=self.buttons[1]
            if down_state==1:
                if not has_sent:
                    rospy.loginfo("履带蛇低头")
                    self.robot.move_joint_speed(3,-1)
                    has_sent=True
            rospy.sleep(SLEEP_TIME)

    def button_handler(self):
        rospy.loginfo("输入控制线程已启动（按钮 + 摇杆轴）")
        
        last_button_actions = {btn_id: None for btn_id in self.button_actions}
        last_axis_actions = {
            axis_id: {value: None for value in actions}
            for axis_id, actions in self.axis_actions.items()
        }
        
        while not rospy.is_shutdown():
            # 处理按钮事件
            for btn_id, action in self.button_actions.items():
                if action["active"] and last_button_actions[btn_id] != action["name"]:
                    action["handler"]()
                    last_button_actions[btn_id] = action["name"]
                elif not action["active"]:
                    last_button_actions[btn_id] = None
            
            # 处理摇杆轴事件
            for axis_id, actions in self.axis_actions.items():
                for value, action in actions.items():
                    if action["active"] and last_axis_actions[axis_id][value] != action["name"]:
                        action["handler"]()
                        last_axis_actions[axis_id][value] = action["name"]
                    elif not action["active"]:
                        last_axis_actions[axis_id][value] = None
            
            rospy.sleep(SLEEP_TIME)

    def Disable(self):
        rospy.loginfo("电机失能")
        self.robot.disable_all()
    def Enable(self):
        rospy.loginfo("电机使能")
        self.robot.enable_all()

    def SnakeToStraight(self):
        rospy.loginfo("一键拉直")
        self.robot.move_straight()

    def Mode(self):
        if not self.Mode_state:
            self.robot.Position_mode()
            rospy.loginfo("切换位置模式")
        else:
            self.robot.Velocity_mode()
            rospy.loginfo("切换速度模式")
        self.Mode_state=not self.Mode_state

    def Tail(self):
        if not self.Tail_state:
            rospy.loginfo("尾门关闭")
        else:
            rospy.loginfo("尾门开启")
        self.Tail_state=not self.Tail_state


    def LED(self):
        if not self.LED_state:
            self.led.off()
            rospy.loginfo("LED关灯")
        else:
            self.led.on()
            rospy.loginfo("LED开灯")
        self.LED_state=not self.LED_state

    def Joint_Up(self):
        rospy.loginfo(f"抬头{self.ANGLE}度")
        #three_angle=self.robot.get_angle_data(3)#获取3号电机当前的角度
        three_angle=self.robot.joint3_angle
        three_angle+=self.ANGLE#增加角度
        self.robot.move_joint_angle(3,three_angle)

    def Joint_Down(self):
        rospy.loginfo(f"低头{self.ANGLE}度")
        #three_angle=self.robot.get_angle_data(3)
        three_angle=self.robot.joint3_angle
        three_angle-=self.ANGLE#减少角度
        self.robot.move_joint_angle(3,three_angle)

    def Joint_Left(self):
        rospy.loginfo(f"左转{self.ANGLE}度")
        #two_angle=self.robot.get_angle_data(2)
        two_angle=self.robot.joint2_angle
        two_angle-=self.ANGLE
        self.robot.move_joint_angle(2,two_angle)

    def Joint_Right(self):
        rospy.loginfo(f"右转{self.ANGLE}度")
        #two_angle=self.robot.get_angle_data(2)
        two_angle=self.robot.joint2_angle
        two_angle+=self.ANGLE
        self.robot.move_joint_angle(2,two_angle)

    def shutdown(self):
        """关闭时的清理操作"""
        rospy.loginfo("正在关闭节点...")
        self.running = False
        self.robot.is_running=False
        
        if self.frontback_thread:
            self.frontback_thread.join(timeout=1.0)
        if self.leftright_thread:
            self.leftright_thread.join(timeout=1.0)
        if self.up_thread:
            self.up_thread.join(timeout=1.0)
        if self.down_thread:
            self.down_thread.join(timeout=1.0)
        if self.button_thread:
            self.button_thread.join(timeout=1.0)
        rospy.loginfo("节点已关闭")

if __name__ == '__main__':
    try:
        demo = JoystickDemo()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass